%************************************************
% Verify whether the pose calculated from 
% the six joint angles is consistent with the pose 
% obtained from the teaching pendant.
%
%
%************************************************

%% Import data and model
data = readmatrix("ur5_kwr75B.xlsx", 'Range', 'B3:M12'); 
std_ur5
%% Calculate pose from 6 joint angles
T = ur5.fkine(deg2rad(data(:, 1:6)));
rotation_matrix_by_joints = T(1).R % print first pose
%% Calculate pose from rotation vector and position
rotation_vector = data(:, 10:12);
axis_angle_1 = [rotation_vector(1, :)/norm(rotation_vector(1, :)), norm(rotation_vector(1, :))];
rotation_matrix_by_vector = axang2rotm(axis_angle_1) % print first pose
